#include "gtest/gtest.h"
#include "TDBase.h"

TEST (PathFollower, Follow)
{
	StageMap map;
	PathRecyclePool pool;

	//	+--+--+--+--+
	//	| 0| 1| 2| 3|
	//	+--+--+--+--+  +--+
	//	| 4|  | 5| 6|  | 7|
	//	+--+  +--+--+  +--+
	
	map.Clear();
	map.Append(MapBrick(0,vector3di( 0,0,4),0,false,true,-1, 1, 4,-1));
	map.Append(MapBrick(0,vector3di( 2,0,4),0,false,true,-1, 2,-1, 0));
	map.Append(MapBrick(0,vector3di( 4,0,4),0,false,true,-1, 3, 5, 1));
	map.Append(MapBrick(0,vector3di( 6,0,4),0,false,true,-1,-1, 6, 2));
	map.Append(MapBrick(0,vector3di( 0,0,2),0,false,true, 0,-1,-1,-1));
	map.Append(MapBrick(0,vector3di( 4,0,2),0,false,true, 2, 6,-1,-1));
	map.Append(MapBrick(0,vector3di( 6,0,2),0,false,true,-1,-1, 5, 3));
	map.Append(MapBrick(0,vector3di(10,0,2),0,false,true,-1,-1,-1,-1));

	PathInfo pathInfo = pool.FindPath(map, 4,5);
	ASSERT_EQ(0, pathInfo.PathIdx);
	ASSERT_EQ(0, pathInfo.SegIdx);

	PathFollower follower;
	follower.Init(&pool, pathInfo.PathIdx, vector3di(0, 0, 2), pathInfo.SegIdx);
	
	ASSERT_EQ (vector3df(0, 0, 2), follower.Position());

	float totalLen = 2.0f +1.0f * 2.0f + sqrt(2.0f) *2.0f;
	float step = 0.15f;
	
	for (float s=0.0f; s< totalLen -step; s+= step)
	{
		bool arriveEnd =follower.Forward(step);
		ASSERT_FALSE(arriveEnd);
	}

	ASSERT_TRUE(follower.Forward(step));
}

TEST (PathFollower, Follow_from_middle)
{
	StageMap map;
	PathRecyclePool pool;

	//	+--+--+--+--+
	//	| 0| 1| 2| 3|
	//	+--+--+--+--+  +--+
	//	| 4|  | 5| 6|  | 7|
	//	+--+  +--+--+  +--+
	
	map.Clear();
	map.Append(MapBrick(0,vector3di( 0,0,4),0,false,true,-1, 1, 4,-1));
	map.Append(MapBrick(0,vector3di( 2,0,4),0,false,true,-1, 2,-1, 0));
	map.Append(MapBrick(0,vector3di( 4,0,4),0,false,true,-1, 3, 5, 1));
	map.Append(MapBrick(0,vector3di( 6,0,4),0,false,true,-1,-1, 6, 2));
	map.Append(MapBrick(0,vector3di( 0,0,2),0,false,true, 0,-1,-1,-1));
	map.Append(MapBrick(0,vector3di( 4,0,2),0,false,true, 2, 6,-1,-1));
	map.Append(MapBrick(0,vector3di( 6,0,2),0,false,true,-1,-1, 5, 3));
	map.Append(MapBrick(0,vector3di(10,0,2),0,false,true,-1,-1,-1,-1));

	PathInfo pathInfo = pool.FindPath(map, 4,5);
	ASSERT_EQ(0, pathInfo.PathIdx);
	ASSERT_EQ(0, pathInfo.SegIdx);

	PathFollower follower;
	follower.Init(&pool, pathInfo.PathIdx, vector3di(0, 0, 4), 1);
	
	ASSERT_EQ (vector3df(0, 0, 4), follower.Position());

	float totalLen = 2.0f +1.0f * 2.0f + sqrt(2.0f) *1.0f;
	float step = 0.15f;
	
	for (float s=0.0f; s< totalLen -step; s+= step)
	{
		bool arriveEnd =follower.Forward(step);
		ASSERT_FALSE(arriveEnd);
	}

	ASSERT_TRUE(follower.Forward(step));
}

TEST (PathFollower, Follow_from_end)
{
	StageMap map;
	PathRecyclePool pool;

	//	+--+--+--+--+
	//	| 0| 1| 2| 3|
	//	+--+--+--+--+  +--+
	//	| 4|  | 5| 6|  | 7|
	//	+--+  +--+--+  +--+
	
	map.Clear();
	map.Append(MapBrick(0,vector3di( 0,0,4),0,false,true,-1, 1, 4,-1));
	map.Append(MapBrick(0,vector3di( 2,0,4),0,false,true,-1, 2,-1, 0));
	map.Append(MapBrick(0,vector3di( 4,0,4),0,false,true,-1, 3, 5, 1));
	map.Append(MapBrick(0,vector3di( 6,0,4),0,false,true,-1,-1, 6, 2));
	map.Append(MapBrick(0,vector3di( 0,0,2),0,false,true, 0,-1,-1,-1));
	map.Append(MapBrick(0,vector3di( 4,0,2),0,false,true, 2, 6,-1,-1));
	map.Append(MapBrick(0,vector3di( 6,0,2),0,false,true,-1,-1, 5, 3));
	map.Append(MapBrick(0,vector3di(10,0,2),0,false,true,-1,-1,-1,-1));

	PathInfo pathInfo = pool.FindPath(map, 4,5);
	ASSERT_EQ(0, pathInfo.PathIdx);
	ASSERT_EQ(0, pathInfo.SegIdx);

	PathFollower follower;

	follower.Init(&pool, pathInfo.PathIdx, vector3di(4, 0, 2), 4);
	
	ASSERT_EQ (vector3df(4, 0, 2), follower.Position());
	ASSERT_TRUE(follower.Forward(0.0001f));
}

TEST (PathFollower, Follow_one_node_path)
{
	StageMap map;
	PathRecyclePool pool;

	//	+--+--+--+--+
	//	| 0| 1| 2| 3|
	//	+--+--+--+--+  +--+
	//	| 4|  | 5| 6|  | 7|
	//	+--+  +--+--+  +--+

	map.Clear();
	map.Append(MapBrick(0,vector3di( 0,0,4),0,false,true,-1, 1, 4,-1));
	map.Append(MapBrick(0,vector3di( 2,0,4),0,false,true,-1, 2,-1, 0));
	map.Append(MapBrick(0,vector3di( 4,0,4),0,false,true,-1, 3, 5, 1));
	map.Append(MapBrick(0,vector3di( 6,0,4),0,false,true,-1,-1, 6, 2));
	map.Append(MapBrick(0,vector3di( 0,0,2),0,false,true, 0,-1,-1,-1));
	map.Append(MapBrick(0,vector3di( 4,0,2),0,false,true, 2, 6,-1,-1));
	map.Append(MapBrick(0,vector3di( 6,0,2),0,false,true,-1,-1, 5, 3));
	map.Append(MapBrick(0,vector3di(10,0,2),0,false,true,-1,-1,-1,-1));

	PathInfo pathInfo = pool.FindPath(map, 0,0);
	ASSERT_EQ(0, pathInfo.PathIdx);
	ASSERT_EQ(0, pathInfo.SegIdx);

	PathFollower follower;
	follower.Init(&pool, pathInfo.PathIdx, vector3di(0, 0, 3), 0);
	
	ASSERT_EQ (vector3df(0, 0, 3), follower.Position());

	float totalLen = 1.0f;
	float step = 0.15f;
	
	for (float s=0.0f; s< totalLen -step; s+= step)
	{
		bool arriveEnd =follower.Forward(step);
		ASSERT_FALSE(arriveEnd);
	}

	ASSERT_TRUE(follower.Forward(step));
}